home *** CD-ROM | disk | FTP | other *** search
/ Champak 141 / (Vol 141) Oct 17 2011.iso / Games / despereaux-swings.swf / scripts / Box2D / Dynamics / Joints / b2RevoluteJoint.as < prev    next >
Encoding:
Text File  |  2011-10-17  |  17.6 KB  |  478 lines
  1. package Box2D.Dynamics.Joints
  2. {
  3.    import Box2D.Common.Math.b2Mat22;
  4.    import Box2D.Common.Math.b2Math;
  5.    import Box2D.Common.Math.b2Vec2;
  6.    import Box2D.Common.b2Settings;
  7.    import Box2D.Dynamics.b2Body;
  8.    import Box2D.Dynamics.b2TimeStep;
  9.    
  10.    public class b2RevoluteJoint extends b2Joint
  11.    {
  12.       
  13.       public static var tImpulse:b2Vec2 = new b2Vec2();
  14.        
  15.       
  16.       public var m_limitForce:Number;
  17.       
  18.       public var m_motorForce:Number;
  19.       
  20.       public var m_pivotMass:b2Mat22;
  21.       
  22.       public var m_enableLimit:Boolean;
  23.       
  24.       public var m_limitState:int;
  25.       
  26.       public var m_motorMass:Number;
  27.       
  28.       public var m_localAnchor1:b2Vec2;
  29.       
  30.       public var m_localAnchor2:b2Vec2;
  31.       
  32.       private var K1:b2Mat22;
  33.       
  34.       private var K2:b2Mat22;
  35.       
  36.       private var K3:b2Mat22;
  37.       
  38.       private var K:b2Mat22;
  39.       
  40.       public var m_pivotForce:b2Vec2;
  41.       
  42.       public var m_motorSpeed:Number;
  43.       
  44.       public var m_enableMotor:Boolean;
  45.       
  46.       public var m_limitPositionImpulse:Number;
  47.       
  48.       public var m_maxMotorTorque:Number;
  49.       
  50.       public var m_referenceAngle:Number;
  51.       
  52.       public var m_lowerAngle:Number;
  53.       
  54.       public var m_upperAngle:Number;
  55.       
  56.       public function b2RevoluteJoint(param1:b2RevoluteJointDef)
  57.       {
  58.          K = new b2Mat22();
  59.          K1 = new b2Mat22();
  60.          K2 = new b2Mat22();
  61.          K3 = new b2Mat22();
  62.          m_localAnchor1 = new b2Vec2();
  63.          m_localAnchor2 = new b2Vec2();
  64.          m_pivotForce = new b2Vec2();
  65.          m_pivotMass = new b2Mat22();
  66.          super(param1);
  67.          m_localAnchor1.SetV(param1.localAnchor1);
  68.          m_localAnchor2.SetV(param1.localAnchor2);
  69.          m_referenceAngle = param1.referenceAngle;
  70.          m_pivotForce.Set(0,0);
  71.          m_motorForce = 0;
  72.          m_limitForce = 0;
  73.          m_limitPositionImpulse = 0;
  74.          m_lowerAngle = param1.lowerAngle;
  75.          m_upperAngle = param1.upperAngle;
  76.          m_maxMotorTorque = param1.maxMotorTorque;
  77.          m_motorSpeed = param1.motorSpeed;
  78.          m_enableLimit = param1.enableLimit;
  79.          m_enableMotor = param1.enableMotor;
  80.       }
  81.       
  82.       override public function GetAnchor1() : b2Vec2
  83.       {
  84.          return m_body1.GetWorldPoint(m_localAnchor1);
  85.       }
  86.       
  87.       override public function GetAnchor2() : b2Vec2
  88.       {
  89.          return m_body2.GetWorldPoint(m_localAnchor2);
  90.       }
  91.       
  92.       public function EnableMotor(param1:Boolean) : void
  93.       {
  94.          m_enableMotor = param1;
  95.       }
  96.       
  97.       public function GetUpperLimit() : Number
  98.       {
  99.          return m_upperAngle;
  100.       }
  101.       
  102.       public function GetLowerLimit() : Number
  103.       {
  104.          return m_lowerAngle;
  105.       }
  106.       
  107.       public function SetLimits(param1:Number, param2:Number) : void
  108.       {
  109.          m_lowerAngle = param1;
  110.          m_upperAngle = param2;
  111.       }
  112.       
  113.       public function GetMotorSpeed() : Number
  114.       {
  115.          return m_motorSpeed;
  116.       }
  117.       
  118.       override public function GetReactionForce() : b2Vec2
  119.       {
  120.          return m_pivotForce;
  121.       }
  122.       
  123.       override public function SolvePositionConstraints() : Boolean
  124.       {
  125.          var _loc1_:Number = NaN;
  126.          var _loc2_:Number = NaN;
  127.          var _loc3_:b2Body = null;
  128.          var _loc4_:b2Body = null;
  129.          var _loc5_:Number = NaN;
  130.          var _loc6_:b2Mat22 = null;
  131.          var _loc7_:Number = NaN;
  132.          var _loc8_:Number = NaN;
  133.          var _loc9_:Number = NaN;
  134.          var _loc10_:Number = NaN;
  135.          var _loc11_:Number = NaN;
  136.          var _loc12_:Number = NaN;
  137.          var _loc13_:Number = NaN;
  138.          var _loc14_:Number = NaN;
  139.          var _loc15_:Number = NaN;
  140.          var _loc16_:Number = NaN;
  141.          var _loc17_:Number = NaN;
  142.          var _loc18_:Number = NaN;
  143.          var _loc19_:Number = NaN;
  144.          var _loc20_:Number = NaN;
  145.          var _loc21_:Number = NaN;
  146.          var _loc22_:Number = NaN;
  147.          var _loc23_:Number = NaN;
  148.          var _loc24_:Number = NaN;
  149.          var _loc25_:Number = NaN;
  150.          var _loc26_:Number = NaN;
  151.          _loc3_ = m_body1;
  152.          _loc4_ = m_body2;
  153.          _loc5_ = 0;
  154.          _loc6_ = _loc3_.m_xf.R;
  155.          _loc7_ = m_localAnchor1.x - _loc3_.m_sweep.localCenter.x;
  156.          _loc8_ = m_localAnchor1.y - _loc3_.m_sweep.localCenter.y;
  157.          _loc9_ = _loc6_.col1.x * _loc7_ + _loc6_.col2.x * _loc8_;
  158.          _loc8_ = _loc6_.col1.y * _loc7_ + _loc6_.col2.y * _loc8_;
  159.          _loc7_ = _loc9_;
  160.          _loc6_ = _loc4_.m_xf.R;
  161.          _loc10_ = m_localAnchor2.x - _loc4_.m_sweep.localCenter.x;
  162.          _loc11_ = m_localAnchor2.y - _loc4_.m_sweep.localCenter.y;
  163.          _loc9_ = _loc6_.col1.x * _loc10_ + _loc6_.col2.x * _loc11_;
  164.          _loc11_ = _loc6_.col1.y * _loc10_ + _loc6_.col2.y * _loc11_;
  165.          _loc10_ = _loc9_;
  166.          _loc12_ = _loc3_.m_sweep.c.x + _loc7_;
  167.          _loc13_ = _loc3_.m_sweep.c.y + _loc8_;
  168.          _loc14_ = _loc4_.m_sweep.c.x + _loc10_;
  169.          _loc15_ = _loc4_.m_sweep.c.y + _loc11_;
  170.          _loc16_ = _loc14_ - _loc12_;
  171.          _loc17_ = _loc15_ - _loc13_;
  172.          _loc5_ = Math.sqrt(_loc16_ * _loc16_ + _loc17_ * _loc17_);
  173.          _loc18_ = _loc3_.m_invMass;
  174.          _loc19_ = _loc4_.m_invMass;
  175.          _loc20_ = _loc3_.m_invI;
  176.          _loc21_ = _loc4_.m_invI;
  177.          K1.col1.x = _loc18_ + _loc19_;
  178.          K1.col2.x = 0;
  179.          K1.col1.y = 0;
  180.          K1.col2.y = _loc18_ + _loc19_;
  181.          K2.col1.x = _loc20_ * _loc8_ * _loc8_;
  182.          K2.col2.x = -_loc20_ * _loc7_ * _loc8_;
  183.          K2.col1.y = -_loc20_ * _loc7_ * _loc8_;
  184.          K2.col2.y = _loc20_ * _loc7_ * _loc7_;
  185.          K3.col1.x = _loc21_ * _loc11_ * _loc11_;
  186.          K3.col2.x = -_loc21_ * _loc10_ * _loc11_;
  187.          K3.col1.y = -_loc21_ * _loc10_ * _loc11_;
  188.          K3.col2.y = _loc21_ * _loc10_ * _loc10_;
  189.          K.SetM(K1);
  190.          K.AddM(K2);
  191.          K.AddM(K3);
  192.          K.Solve(tImpulse,-_loc16_,-_loc17_);
  193.          _loc22_ = tImpulse.x;
  194.          _loc23_ = tImpulse.y;
  195.          _loc3_.m_sweep.c.x -= _loc3_.m_invMass * _loc22_;
  196.          _loc3_.m_sweep.c.y -= _loc3_.m_invMass * _loc23_;
  197.          _loc3_.m_sweep.a -= _loc3_.m_invI * (_loc7_ * _loc23_ - _loc8_ * _loc22_);
  198.          _loc4_.m_sweep.c.x += _loc4_.m_invMass * _loc22_;
  199.          _loc4_.m_sweep.c.y += _loc4_.m_invMass * _loc23_;
  200.          _loc4_.m_sweep.a += _loc4_.m_invI * (_loc10_ * _loc23_ - _loc11_ * _loc22_);
  201.          _loc3_.SynchronizeTransform();
  202.          _loc4_.SynchronizeTransform();
  203.          _loc24_ = 0;
  204.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  205.          {
  206.             _loc25_ = _loc4_.m_sweep.a - _loc3_.m_sweep.a - m_referenceAngle;
  207.             _loc26_ = 0;
  208.             if(m_limitState == e_equalLimits)
  209.             {
  210.                _loc2_ = b2Math.b2Clamp(_loc25_,-b2Settings.b2_maxAngularCorrection,b2Settings.b2_maxAngularCorrection);
  211.                _loc26_ = -m_motorMass * _loc2_;
  212.                _loc24_ = b2Math.b2Abs(_loc2_);
  213.             }
  214.             else if(m_limitState == e_atLowerLimit)
  215.             {
  216.                _loc2_ = _loc25_ - m_lowerAngle;
  217.                _loc24_ = b2Math.b2Max(0,-_loc2_);
  218.                _loc2_ = b2Math.b2Clamp(_loc2_ + b2Settings.b2_angularSlop,-b2Settings.b2_maxAngularCorrection,0);
  219.                _loc26_ = -m_motorMass * _loc2_;
  220.                _loc1_ = m_limitPositionImpulse;
  221.                m_limitPositionImpulse = b2Math.b2Max(m_limitPositionImpulse + _loc26_,0);
  222.                _loc26_ = m_limitPositionImpulse - _loc1_;
  223.             }
  224.             else if(m_limitState == e_atUpperLimit)
  225.             {
  226.                _loc2_ = _loc25_ - m_upperAngle;
  227.                _loc24_ = b2Math.b2Max(0,_loc2_);
  228.                _loc2_ = b2Math.b2Clamp(_loc2_ - b2Settings.b2_angularSlop,0,b2Settings.b2_maxAngularCorrection);
  229.                _loc26_ = -m_motorMass * _loc2_;
  230.                _loc1_ = m_limitPositionImpulse;
  231.                m_limitPositionImpulse = b2Math.b2Min(m_limitPositionImpulse + _loc26_,0);
  232.                _loc26_ = m_limitPositionImpulse - _loc1_;
  233.             }
  234.             _loc3_.m_sweep.a -= _loc3_.m_invI * _loc26_;
  235.             _loc4_.m_sweep.a += _loc4_.m_invI * _loc26_;
  236.             _loc3_.SynchronizeTransform();
  237.             _loc4_.SynchronizeTransform();
  238.          }
  239.          return _loc5_ <= b2Settings.b2_linearSlop && _loc24_ <= b2Settings.b2_angularSlop;
  240.       }
  241.       
  242.       public function GetJointSpeed() : Number
  243.       {
  244.          return m_body2.m_angularVelocity - m_body1.m_angularVelocity;
  245.       }
  246.       
  247.       public function SetMotorSpeed(param1:Number) : void
  248.       {
  249.          m_motorSpeed = param1;
  250.       }
  251.       
  252.       public function SetMaxMotorTorque(param1:Number) : void
  253.       {
  254.          m_maxMotorTorque = param1;
  255.       }
  256.       
  257.       public function GetJointAngle() : Number
  258.       {
  259.          return m_body2.m_sweep.a - m_body1.m_sweep.a - m_referenceAngle;
  260.       }
  261.       
  262.       override public function InitVelocityConstraints(param1:b2TimeStep) : void
  263.       {
  264.          var _loc2_:b2Body = null;
  265.          var _loc3_:b2Body = null;
  266.          var _loc4_:b2Mat22 = null;
  267.          var _loc5_:Number = NaN;
  268.          var _loc6_:Number = NaN;
  269.          var _loc7_:Number = NaN;
  270.          var _loc8_:Number = NaN;
  271.          var _loc9_:Number = NaN;
  272.          var _loc10_:Number = NaN;
  273.          var _loc11_:Number = NaN;
  274.          var _loc12_:Number = NaN;
  275.          var _loc13_:Number = NaN;
  276.          var _loc14_:Number = NaN;
  277.          _loc2_ = m_body1;
  278.          _loc3_ = m_body2;
  279.          _loc4_ = _loc2_.m_xf.R;
  280.          _loc6_ = m_localAnchor1.x - _loc2_.m_sweep.localCenter.x;
  281.          _loc7_ = m_localAnchor1.y - _loc2_.m_sweep.localCenter.y;
  282.          _loc5_ = _loc4_.col1.x * _loc6_ + _loc4_.col2.x * _loc7_;
  283.          _loc7_ = _loc4_.col1.y * _loc6_ + _loc4_.col2.y * _loc7_;
  284.          _loc6_ = _loc5_;
  285.          _loc4_ = _loc3_.m_xf.R;
  286.          _loc8_ = m_localAnchor2.x - _loc3_.m_sweep.localCenter.x;
  287.          _loc9_ = m_localAnchor2.y - _loc3_.m_sweep.localCenter.y;
  288.          _loc5_ = _loc4_.col1.x * _loc8_ + _loc4_.col2.x * _loc9_;
  289.          _loc9_ = _loc4_.col1.y * _loc8_ + _loc4_.col2.y * _loc9_;
  290.          _loc8_ = _loc5_;
  291.          _loc10_ = _loc2_.m_invMass;
  292.          _loc11_ = _loc3_.m_invMass;
  293.          _loc12_ = _loc2_.m_invI;
  294.          _loc13_ = _loc3_.m_invI;
  295.          K1.col1.x = _loc10_ + _loc11_;
  296.          K1.col2.x = 0;
  297.          K1.col1.y = 0;
  298.          K1.col2.y = _loc10_ + _loc11_;
  299.          K2.col1.x = _loc12_ * _loc7_ * _loc7_;
  300.          K2.col2.x = -_loc12_ * _loc6_ * _loc7_;
  301.          K2.col1.y = -_loc12_ * _loc6_ * _loc7_;
  302.          K2.col2.y = _loc12_ * _loc6_ * _loc6_;
  303.          K3.col1.x = _loc13_ * _loc9_ * _loc9_;
  304.          K3.col2.x = -_loc13_ * _loc8_ * _loc9_;
  305.          K3.col1.y = -_loc13_ * _loc8_ * _loc9_;
  306.          K3.col2.y = _loc13_ * _loc8_ * _loc8_;
  307.          K.SetM(K1);
  308.          K.AddM(K2);
  309.          K.AddM(K3);
  310.          K.Invert(m_pivotMass);
  311.          m_motorMass = 1 / (_loc12_ + _loc13_);
  312.          if(m_enableMotor == false)
  313.          {
  314.             m_motorForce = 0;
  315.          }
  316.          if(m_enableLimit)
  317.          {
  318.             _loc14_ = _loc3_.m_sweep.a - _loc2_.m_sweep.a - m_referenceAngle;
  319.             if(b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2 * b2Settings.b2_angularSlop)
  320.             {
  321.                m_limitState = e_equalLimits;
  322.             }
  323.             else if(_loc14_ <= m_lowerAngle)
  324.             {
  325.                if(m_limitState != e_atLowerLimit)
  326.                {
  327.                   m_limitForce = 0;
  328.                }
  329.                m_limitState = e_atLowerLimit;
  330.             }
  331.             else if(_loc14_ >= m_upperAngle)
  332.             {
  333.                if(m_limitState != e_atUpperLimit)
  334.                {
  335.                   m_limitForce = 0;
  336.                }
  337.                m_limitState = e_atUpperLimit;
  338.             }
  339.             else
  340.             {
  341.                m_limitState = e_inactiveLimit;
  342.                m_limitForce = 0;
  343.             }
  344.          }
  345.          else
  346.          {
  347.             m_limitForce = 0;
  348.          }
  349.          if(param1.warmStarting)
  350.          {
  351.             _loc2_.m_linearVelocity.x -= param1.dt * _loc10_ * m_pivotForce.x;
  352.             _loc2_.m_linearVelocity.y -= param1.dt * _loc10_ * m_pivotForce.y;
  353.             _loc2_.m_angularVelocity -= param1.dt * _loc12_ * (_loc6_ * m_pivotForce.y - _loc7_ * m_pivotForce.x + m_motorForce + m_limitForce);
  354.             _loc3_.m_linearVelocity.x += param1.dt * _loc11_ * m_pivotForce.x;
  355.             _loc3_.m_linearVelocity.y += param1.dt * _loc11_ * m_pivotForce.y;
  356.             _loc3_.m_angularVelocity += param1.dt * _loc13_ * (_loc8_ * m_pivotForce.y - _loc9_ * m_pivotForce.x + m_motorForce + m_limitForce);
  357.          }
  358.          else
  359.          {
  360.             m_pivotForce.SetZero();
  361.             m_motorForce = 0;
  362.             m_limitForce = 0;
  363.          }
  364.          m_limitPositionImpulse = 0;
  365.       }
  366.       
  367.       public function EnableLimit(param1:Boolean) : void
  368.       {
  369.          m_enableLimit = param1;
  370.       }
  371.       
  372.       public function GetMotorTorque() : Number
  373.       {
  374.          return m_motorForce;
  375.       }
  376.       
  377.       override public function GetReactionTorque() : Number
  378.       {
  379.          return m_limitForce;
  380.       }
  381.       
  382.       public function IsLimitEnabled() : Boolean
  383.       {
  384.          return m_enableLimit;
  385.       }
  386.       
  387.       public function IsMotorEnabled() : Boolean
  388.       {
  389.          return m_enableMotor;
  390.       }
  391.       
  392.       override public function SolveVelocityConstraints(param1:b2TimeStep) : void
  393.       {
  394.          var _loc2_:b2Body = null;
  395.          var _loc3_:b2Body = null;
  396.          var _loc4_:b2Mat22 = null;
  397.          var _loc5_:Number = NaN;
  398.          var _loc6_:Number = NaN;
  399.          var _loc7_:Number = NaN;
  400.          var _loc8_:Number = NaN;
  401.          var _loc9_:Number = NaN;
  402.          var _loc10_:Number = NaN;
  403.          var _loc11_:Number = NaN;
  404.          var _loc12_:Number = NaN;
  405.          var _loc13_:Number = NaN;
  406.          var _loc14_:Number = NaN;
  407.          var _loc15_:Number = NaN;
  408.          var _loc16_:Number = NaN;
  409.          var _loc17_:Number = NaN;
  410.          var _loc18_:Number = NaN;
  411.          var _loc19_:Number = NaN;
  412.          var _loc20_:Number = NaN;
  413.          var _loc21_:Number = NaN;
  414.          _loc2_ = m_body1;
  415.          _loc3_ = m_body2;
  416.          _loc4_ = _loc2_.m_xf.R;
  417.          _loc6_ = m_localAnchor1.x - _loc2_.m_sweep.localCenter.x;
  418.          _loc7_ = m_localAnchor1.y - _loc2_.m_sweep.localCenter.y;
  419.          _loc5_ = _loc4_.col1.x * _loc6_ + _loc4_.col2.x * _loc7_;
  420.          _loc7_ = _loc4_.col1.y * _loc6_ + _loc4_.col2.y * _loc7_;
  421.          _loc6_ = _loc5_;
  422.          _loc4_ = _loc3_.m_xf.R;
  423.          _loc8_ = m_localAnchor2.x - _loc3_.m_sweep.localCenter.x;
  424.          _loc9_ = m_localAnchor2.y - _loc3_.m_sweep.localCenter.y;
  425.          _loc5_ = _loc4_.col1.x * _loc8_ + _loc4_.col2.x * _loc9_;
  426.          _loc9_ = _loc4_.col1.y * _loc8_ + _loc4_.col2.y * _loc9_;
  427.          _loc8_ = _loc5_;
  428.          _loc11_ = _loc3_.m_linearVelocity.x + -_loc3_.m_angularVelocity * _loc9_ - _loc2_.m_linearVelocity.x - -_loc2_.m_angularVelocity * _loc7_;
  429.          _loc12_ = _loc3_.m_linearVelocity.y + _loc3_.m_angularVelocity * _loc8_ - _loc2_.m_linearVelocity.y - _loc2_.m_angularVelocity * _loc6_;
  430.          _loc13_ = -param1.inv_dt * (m_pivotMass.col1.x * _loc11_ + m_pivotMass.col2.x * _loc12_);
  431.          _loc14_ = -param1.inv_dt * (m_pivotMass.col1.y * _loc11_ + m_pivotMass.col2.y * _loc12_);
  432.          m_pivotForce.x += _loc13_;
  433.          m_pivotForce.y += _loc14_;
  434.          _loc15_ = param1.dt * _loc13_;
  435.          _loc16_ = param1.dt * _loc14_;
  436.          _loc2_.m_linearVelocity.x -= _loc2_.m_invMass * _loc15_;
  437.          _loc2_.m_linearVelocity.y -= _loc2_.m_invMass * _loc16_;
  438.          _loc2_.m_angularVelocity -= _loc2_.m_invI * (_loc6_ * _loc16_ - _loc7_ * _loc15_);
  439.          _loc3_.m_linearVelocity.x += _loc3_.m_invMass * _loc15_;
  440.          _loc3_.m_linearVelocity.y += _loc3_.m_invMass * _loc16_;
  441.          _loc3_.m_angularVelocity += _loc3_.m_invI * (_loc8_ * _loc16_ - _loc9_ * _loc15_);
  442.          if(m_enableMotor && m_limitState != e_equalLimits)
  443.          {
  444.             _loc17_ = _loc3_.m_angularVelocity - _loc2_.m_angularVelocity - m_motorSpeed;
  445.             _loc18_ = -param1.inv_dt * m_motorMass * _loc17_;
  446.             _loc19_ = m_motorForce;
  447.             m_motorForce = b2Math.b2Clamp(m_motorForce + _loc18_,-m_maxMotorTorque,m_maxMotorTorque);
  448.             _loc18_ = m_motorForce - _loc19_;
  449.             _loc2_.m_angularVelocity -= _loc2_.m_invI * param1.dt * _loc18_;
  450.             _loc3_.m_angularVelocity += _loc3_.m_invI * param1.dt * _loc18_;
  451.          }
  452.          if(m_enableLimit && m_limitState != e_inactiveLimit)
  453.          {
  454.             _loc20_ = _loc3_.m_angularVelocity - _loc2_.m_angularVelocity;
  455.             _loc21_ = -param1.inv_dt * m_motorMass * _loc20_;
  456.             if(m_limitState == e_equalLimits)
  457.             {
  458.                m_limitForce += _loc21_;
  459.             }
  460.             else if(m_limitState == e_atLowerLimit)
  461.             {
  462.                _loc10_ = m_limitForce;
  463.                m_limitForce = b2Math.b2Max(m_limitForce + _loc21_,0);
  464.                _loc21_ = m_limitForce - _loc10_;
  465.             }
  466.             else if(m_limitState == e_atUpperLimit)
  467.             {
  468.                _loc10_ = m_limitForce;
  469.                m_limitForce = b2Math.b2Min(m_limitForce + _loc21_,0);
  470.                _loc21_ = m_limitForce - _loc10_;
  471.             }
  472.             _loc2_.m_angularVelocity -= _loc2_.m_invI * param1.dt * _loc21_;
  473.             _loc3_.m_angularVelocity += _loc3_.m_invI * param1.dt * _loc21_;
  474.          }
  475.       }
  476.    }
  477. }
  478.